import numpy as np
from qpsolvers import solve_qp

def F7_MPC_Controller_withConstraints(x, F, H, M, Beta_bar, b, p):
    
    # 求解二次规划问题
    result = solve_qp(P=H, q=F @ x, G=M, h=Beta_bar + b @ x, A=None, b=None, lb=None, ub=None, solver="osqp")

    # 提取最优控制输入
    U = result
    
    # 根据模型预测控制的策略，仅选取所得输入的第一项
    u = U[:p]

    return U, u
